#!/bin/bash
source /opt/ros/humble/setup.bash
echo "========================================="
echo "机械臂诊断脚本"
echo "========================================="
echo ""
echo "1. 检查话题订阅者..."
ros2 topic info /joint_ctrl_cmd_left
ros2 topic info /joint_ctrl_cmd_right
echo ""
echo "2. 检查节点状态..."
ros2 node list | grep piper
echo ""
echo "3. 测试发送指令到左臂（joint1=1.0）..."
ros2 topic pub -1 /joint_ctrl_cmd_left sensor_msgs/JointState "{name: ['joint1','joint2','joint3','joint4','joint5','joint6','joint7'], position: [1.0,0,0,0,0,0,0], velocity: [0,0,0,0,0,0,80], effort: [0,0,0,0,0,0,0.5]}"
echo ""
echo "4. 测试发送指令到右臂（joint1=1.0）..."
ros2 topic pub -1 /joint_ctrl_cmd_right sensor_msgs/JointState "{name: ['joint1','joint2','joint3','joint4','joint5','joint6','joint7'], position: [1.0,0,0,0,0,0,0], velocity: [0,0,0,0,0,0,80], effort: [0,0,0,0,0,0,0.5]}"
echo ""
echo "5. 查看节点输出，看哪个臂接收到了指令"
echo "========================================="

